/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */




public class RobotTemplate extends IterativeRobot {
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    private Jaguar FrontLeft = null;
    private Jaguar FrontRight = null;
    private Jaguar BackLeft = null;
    private Jaguar BackRight = null;
    private Joystick SimpleStick = null;
    private Gyro TiltGyro = null;
    private double JoyMultiplyer = 1;    
    double TiltResult;
    int ToggleVar;
    double MotorVal;
    float StartAngleSetter;
    double AngleOfDeflectionRamp = 30;

    public void robotInit() {
        FrontLeft = new Jaguar(1);
        FrontRight = new Jaguar(2);
        BackLeft = new Jaguar(3);
        BackRight = new Jaguar(4);
        SimpleStick = new Joystick(1);
        TiltGyro = new Gyro(5);

        

    }
    
    static double limit(double num) {
        if (num > 1.0) {
            return 1.0;
        }
        if (num < -1.0) {
            return -1.0;
        }
        return num;
    }

    public void newArcadeDrive() {
        double leftMotorSpeed;
        double rightMotorSpeed;

        double moveValue = -SimpleStick.getY();
        double rotateValue = -SimpleStick.getX();

        boolean squaredInputs = true;


        moveValue = limit(moveValue);
        rotateValue = limit(rotateValue);

        if (squaredInputs) {
            // square the inputs (while preserving the sign) to increase fine control while permitting full power
            if (moveValue >= 0.0) {
                moveValue = (moveValue * moveValue);
            } else {
                moveValue = -(moveValue * moveValue);
            }
            if (rotateValue >= 0.0) {
                rotateValue = (rotateValue * rotateValue);
            } else {
                rotateValue = -(rotateValue * rotateValue);
            }
        }

        if (moveValue > 0.0) {
            if (rotateValue > 0.0) {
                leftMotorSpeed = moveValue - rotateValue;
                rightMotorSpeed = Math.max(moveValue, rotateValue);
            } else {
                leftMotorSpeed = Math.max(moveValue, -rotateValue);
                rightMotorSpeed = moveValue + rotateValue;
            }
        } else {
            if (rotateValue > 0.0) {
                leftMotorSpeed = -Math.max(-moveValue, rotateValue);
                rightMotorSpeed = moveValue + rotateValue;
            } else {
                leftMotorSpeed = moveValue - rotateValue;
                rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
            }
        }

        FrontLeft.set(-leftMotorSpeed*JoyMultiplyer);
        BackLeft.set(-leftMotorSpeed*JoyMultiplyer);
        FrontRight.set(rightMotorSpeed*JoyMultiplyer);
        BackRight.set(rightMotorSpeed*JoyMultiplyer);
    }
    
    public void SpeedSetter (double SpeedM) {
        FrontLeft.set(-SpeedM);
        BackLeft.set(-SpeedM);
        FrontRight.set(SpeedM);
        BackRight.set(SpeedM);
    }


    

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {

    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        newArcadeDrive();
        if (SimpleStick.getRawButton(11)) {
            JoyMultiplyer = 0.5;
        }
        else{
            JoyMultiplyer = 1;
        }
        /*Begin Gyro Stablize Sudeocode
           Get Gyro Angle (0 as horizontal)
         * 
         * Press button to set which start stablization and also set what number(Postitive or negative) will correspond to
         * backwards or forwards adjustment 
         *
         * Find what max deflection should be and make a constant for that.
         * 
         * Divide the current angle by that constant to get a number from one to zero (To send to the motors for driving)
         * 
         * Possibly devide that number by .75 or so to make the adj. not as harsh
        */ 
        

           if(SimpleStick.getRawButton(12)){
               ToggleVar++;
                   if(ToggleVar == 1){
                           if(TiltGyro.getAngle() > 0){
                                   StartAngleSetter = (float) 0.5;
                           }
                           else if(TiltGyro.getAngle() < 0){
                                   StartAngleSetter = (float) -0.5;
                           }
               }
         
               TiltResult = TiltGyro.getAngle()/AngleOfDeflectionRamp;
               MotorVal = TiltResult*StartAngleSetter;
          
               SpeedSetter(MotorVal);
         }
        else{ToggleVar = 0;}

        
    }
    
}
